import cv2
import numpy as np

COLOR_LOW = np.array([123, 75, 160])
COLOR_HIGH = np.array([179, 255, 255])

LOW = 340
HIGH = 400

T = []

def houghline_blue(img):
    # 转换颜色空间为HSV
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # 定义紫色的范围
    lower_purple = np.array([116, 20, 90])
    upper_purple = np.array([179, 255, 255])

    # 根据阈值提取紫色部分
    mask = cv2.inRange(hsv, lower_purple, upper_purple)

    # 将原始图像与掩码进行按位与运算
    img = cv2.bitwise_and(img, img, mask=mask)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret,gray=cv2.threshold(gray,150,255,cv2.THRESH_TOZERO)
    edges = cv2.Canny(gray, 200, 255)
    lines = cv2.HoughLinesP(edges, 1, np.pi / 180, threshold=40, minLineLength=50, maxLineGap=200)
    zixian=[]
    zixian_aver=[0,0,0,0,0]
    if lines is not None:
        for line in lines:
            x1,y1,x2,y2 =line[0]
            if abs((y1-y2)/(x1-x2))<0.2:
                zixian.append([(y1-y2)/(x1-x2),x1,y1,x2,y2])
                cv2.line(img,(x1,y1),(x2,y2),(0.255,0),1)
        if len(zixian)==0:
            return -1
        else:
            for j in range(5):
                for i in range(len(zixian)):
                    zixian_aver[j]+=zixian[i][j]
                zixian_aver[j]/=len(zixian)
        cv2.line(img,(int(zixian_aver[1]),int(zixian_aver[2])),(int(zixian_aver[3]),int(zixian_aver[4])),(0,0,255),2)
        cv2.imshow("line",img)
        cv2.waitKey(100)
        if (zixian_aver[2]+zixian_aver[4])/2>220:
            return 1
        else:
            return zixian_aver[0]
    else:
        print("no blue line")
        return -1
    

def hough(img):
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    lower_blue = np.array([90, 50, 50])
    upper_blue = np.array([130, 255, 255])
    mask = cv2.inRange(hsv, lower_blue, upper_blue)
    blue_part = cv2.bitwise_and(img, img, mask=mask)
    blue_gray = cv2.cvtColor(blue_part[300:, :, :], cv2.COLOR_BGR2GRAY)
    edges = cv2.Canny(blue_gray, 50, 150, apertureSize=3)
    lines = cv2.HoughLinesP(edges, 1, np.pi / 180, threshold=100, minLineLength=100, maxLineGap=10)
    for line in lines:
        x1, y1, x2, y2 = line[0]
        if x1 == x2:
            continue
        if y1 - y2 == 0 or abs((y1 - y2) / (x1 - x2)) < 0.3:
            global k
            k = abs((y1 - y2) / (x1 - x2))
            return 1
    return 0


##################################################################################################################
def myget_gots(N):
    k = N[0]
    b = N[2] - k * N[1]

    return (240 - b) / k


##################################################################################################################

def houghline(img):
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # kernel = np.ones((3, 3), np.uint8)
    # erosion = cv2.erode(img, kernel, iterations=2)
    # edges = cv2.Canny(erosion, 200, 300, apertureSize=3)
    ret,gray=cv2.threshold(gray,150,255,cv2.THRESH_TOZERO)
    edges = cv2.Canny(gray, 200, 255)
    lines = cv2.HoughLinesP(edges, 1, np.pi / 180, threshold=50, minLineLength=50, maxLineGap=20)
    zheng = [0, 0, 0, 0, 0]
    fu = []
    fu_aver = [0, 0, 0, 0, 0]
    if lines is not None:
        cv2.imshow("edge", edges)
        for line in lines:
            x1, y1, x2, y2 = line[0]
            if x1 == x2:
                continue
            if (y1 - y2) / (x1 - x2) > 0.3 and (y1 - y2) / (x1 - x2) < 1.3 and (y1 - y2) / (x1 - x2) > zheng[0]:
                zheng = [(y1 - y2) / (x1 - x2), x1, y1, x2, y2]
            if (y1 - y2) / (x1 - x2) < -0.3 and (y1 - y2) / (x1 - x2) > -0.8:
                fu.append([(y1 - y2) / (x1 - x2), x1, y1, x2, y2])

        if len(fu) > 0:
            for k in range(5):
                for j in range(len(fu)):
                    fu_aver[k] += fu[j][k]
                fu_aver[k] /= len(fu)
            print("oo")
            cv2.line(img, (int(fu_aver[1]), int(fu_aver[2])), (int(fu_aver[3]), int(fu_aver[4])), (0, 255, 0), 2)
            # print(fu_aver[k])
        # cv2.line(img, (int(zheng_aver[1]), int(zheng_aver[2])), (int(zheng_aver[3]), int(zheng_aver[4])), (0, 0, 255), 2)
        cv2.line(img, (zheng[1], zheng[2]), (zheng[3], zheng[4]), (0, 0, 255), 2)
        cv2.imshow("img", img)
        cv2.waitKey(200)
        ##################################################################################################################
        if fu_aver[0] == 0:
            if zheng[0] == 0:
                return -10000, 10000
            else:
                myright = myget_gots(zheng)
                return -10000, myright
        else:
            myleft = myget_gots(fu_aver)
            if zheng[0] == 0:
                return myleft, 10000
            else:
                myright = myget_gots(zheng)
                return myleft, myright
    else:
        print("no line")
        return -999,-999
        ##################################################################################################################


def change(state):
    if state == 0:
        if k > 0.1:
            return 1
        elif k < -0.1:
            return 5
        else:
            return 3


def strategy(N, state):
    if state == 0:
        if N[2] < 10:
            return 3
        else:
            if N[0] < 30 and N[4] < 30:
                return 3
            else:
                if N[0] > 1000:
                    return 5
                elif N[4] > 1000:
                    return 1
                else:
                    return 3


if __name__ == "__main__":

    cap = cv2.VideoCapture(0)

    while True:
        ret, img = cap.read()
        cv2.imshow("img_orig", img)
        blue_k=houghline_blue(img[240:,:,:])
        if blue_k==1:
            print("refer biaozhi")
            cv2.waitKey(3000)
            continue
        if abs(blue_k-0.01)<=0.02:
            print(blue_k)
            print("0")
        elif blue_k>-0.2 and blue_k<-0.01:
            print(blue_k)
            print("left")
        elif blue_k <0.2 and blue_k >0.03:
            print(blue_k)
            print("right")
        else:
            left, right = houghline(img[240:,:,:])
            print("{}        {}".format(left, right))

            m = 0
            template = 670
            if left == -10000:
                if right == 10000:
                    continue
                else:
                    m = right
                    template = 670
            else:
                if right == 10000:
                    template = -50
                    m = left
                else:
                    template = 310
                    m = (right + left) / 2

            m = m - template
            print(m)

            if m < -50:
                print("l2")
            elif m < -20:
                print("l1")
            elif m > 75:
                print("r1")
            elif m > 130:
                print("r2")
            else:
                print("m")

        cv2.waitKey(100)
